/*
 * mainpp.cpp
 *
 *  Created on: 2018年6月4日
 *      Author: sam
 */

#include "mainpp.h"

#include "motec.h"
#include "adc.h"

/****************************************************************************
*	Function prototypes
****************************************************************************/
void motec_joint_callback(const aibots_robot_msgs::Motec& motec_joint_);
void angle_publish(void);
void motec_joint_feedback(void);

void updateGoalVelocity(void);

/****************************************************************************
*	Extern variable
****************************************************************************/
extern __IO u16 ADC_ConvertedValue;	
extern UNS8 motec_status[2];

/****************************************************************************
*	ROS NodeHandle
****************************************************************************/
ros::NodeHandle nh;
ros::Time control_motor_last_time;
ros::Time sub_vel_last_time;
ros::Time pub_vel_last_time;
ros::Time vel_zore_last_time;

aibots_robot_msgs::Motec goalMotec;

/****************************************************************************
*	Subscriber
****************************************************************************/
ros::Subscriber<aibots_robot_msgs::Motec> motec_sub("motec_joint", motec_joint_callback);

/****************************************************************************
*	Publisher
****************************************************************************/
aibots_robot_msgs::Motec motec_joint_fb_;
ros::Publisher motec_joint_fb_pub("motec_joint_fb", &motec_joint_fb_);

std_msgs::Float64 angle_;
ros::Publisher angle__pub("angle", &angle_);


void setup(void)
{
  nh.initNode();
	
	motec_init();
	
	ADC_Configuration();
	
	nh.advertise(motec_joint_fb_pub);
	nh.advertise(angle__pub);
	nh.subscribe(motec_sub);
	
	control_motor_last_time = nh.now();
	sub_vel_last_time = nh.now();
	pub_vel_last_time = nh.now();
	vel_zore_last_time = nh.now();
}

void loop(void)
{
	ros::Time t = nh.now();
	double dt_1 = t.toSec() - control_motor_last_time.toSec();
	
	if(dt_1>=(1.0/CONTROL_MOTOR_SPEED_FREQUENCY))
	{
		ConfigureSlaveLoop();
		//updateGoalVelocity();
		control_motor_last_time = nh.now();
	}
	//ConfigureSlaveLoop();
	//angle_publish();
	
	double dt_2 = t.toSec() - pub_vel_last_time.toSec();
	if(dt_2>=(1.0/DRIVE_INFORMATION_PUBLISH_FREQUENCY))
	{
		sendSYNCMessage(&ObjDict_Data);
		motec_joint_feedback();
		pub_vel_last_time = nh.now();
	}
	
  nh.spinOnce();
	//delay_ms(30);
}

void motec_joint_callback(const aibots_robot_msgs::Motec& motec_joint_)
{
	
	goalMotec = motec_joint_;
	updateGoalVelocity();
	/*UNS8 isOperation = 1;
	if(motec_status[0] != 0x27)
	{
		if(!Motec_Operation(1))
		{
			isOperation = 0;
			PDOInhibitTimerAlarm(&ObjDict_Data, 0);
		}
	}

	if(motec_status[1] != 0x27)
	{
		if(!Motec_Operation(2))
		{
			isOperation = 0;
			PDOInhibitTimerAlarm(&ObjDict_Data, 1);
		}
	}
	if(isOperation)
	{
		LeftVelocityOut_Velocity = goalMotec.drivers[0];
		sendOnePDOevent(&ObjDict_Data, 0);
		RightVelocityOut_Velocity = goalMotec.drivers[1];
		sendOnePDOevent(&ObjDict_Data, 1);
	}*/
	sub_vel_last_time = nh.now();
}

void angle_publish(void)
{
	angle_.data = ADC_ConvertedValue;
	angle__pub.publish(&angle_);
}

void motec_joint_feedback(void)
{
	motec_joint_fb_.drivers[0] = LeftVelocityIn_Velocity;
	motec_joint_fb_.drivers[1] = RightVelocityIn_Velocity;
	motec_joint_fb_pub.publish(&motec_joint_fb_);
}

/****************************************************************************
*	Update Goal Velocity
****************************************************************************/
void updateGoalVelocity(void)
{
	
	UNS8 isOperation = 1;
	if(motec_status[0] != 0x27)
	{
		if(!Motec_Operation(1))
		{
			isOperation = 0;
			PDOInhibitTimerAlarm(&ObjDict_Data, 0);
		}
	}

	if(motec_status[1] != 0x27)
	{
		if(!Motec_Operation(2))
		{
			isOperation = 0;
			PDOInhibitTimerAlarm(&ObjDict_Data, 1);
		}
	}
	if(isOperation)
	{
		LeftVelocityOut_Velocity = goalMotec.drivers[0];
		sendOnePDOevent(&ObjDict_Data, 0);
		RightVelocityOut_Velocity = goalMotec.drivers[1];
		sendOnePDOevent(&ObjDict_Data, 1);
	}
}